﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Drifting.MotionPlanning.Primitives;
using Drifting.MotionPlanning.Systems;
using Drifting.MotionPlanning.PointSets;
using Drifting.Foundation;

namespace Drifting.MotionPlanning.SingleThreading.Algorithms
{
    abstract public class TreeBasedPlanningAlgorithm
    {
        protected PointSet vertices;
        protected BaseSystem system;
        protected Random rand;
        protected BaseSystem.MemoryPad memoryPad;

        protected State x_root;
        protected GoalCheck inXgoal;
        protected double cost_solution;
        protected State x_goal;

        protected int n_current;
        protected int n_exec;

        protected StringBuilder sb;

        protected DateTime lastCall_time;
        protected int lastCall_iter;
        protected double ips;

        public void clearIPS()
        {
            lastCall_iter = n_current;
            lastCall_time = DateTime.Now;
        }

        public void calculateIPS()
        {
            DateTime time = DateTime.Now;
            if (lastCall_time != null)
            {
                double duration_ms = (time - lastCall_time).TotalMilliseconds;
                int iter_delta = n_current - lastCall_iter;
                ips = (iter_delta / duration_ms * 1000);
            }
            lastCall_iter = n_current;
            lastCall_time = time;
        }



        public State x_solution
        {
            get { return x_goal; }
        }

        virtual public PointSet Vertices
        {
            get { return vertices; }
            set { this.vertices = value; }
        }

        public BaseSystem System
        {
            get { return system; }
        }

        public int iteration_total
        {
            get { return n_exec; }
        }

        public int iteration_current
        {
            get { return n_current; }
        }

        public TreeBasedPlanningAlgorithm(BaseSystem system, Random rand)
        {
            if (rand == null)
                rand = new Random();
            this.rand = rand;

            this.system = system;
            memoryPad = new BaseSystem.MemoryPad();
            memoryPad.Prepare(system.DOF_State);

            x_root = null;
            vertices = null;
            cost_solution = double.MaxValue;
            x_goal = null;

            sb = new StringBuilder(100);
        }

        public void SetGoalChecker(GoalCheck gChecker)
        {
            this.inXgoal = gChecker;
        }

        virtual protected State SpawnState()
        {
            double[] q = null;
            system.SampleState(rand, ref q);
            State x = new State(q, AliasOf(q));

            return x;
        }

        virtual protected State SpawnState(double[] q)
        {
            if (q == null)
                throw new ArgumentException();
            State x = new State(q, AliasOf(q));

            return x;
        }

        virtual protected State SpawnState(double[] q, double[] a)
        {
            if (q == null)
                throw new ArgumentException();
            if (a == null)
                throw new ArgumentException();
            if (q.Length != system.DOF_State)
                throw new ArgumentException();
            if (a.Length != system.DOF_Alias)
                throw new ArgumentException();
            
            State x = new State(q, a);

            return x;
        }

        protected Edge SpawnEdge(State x_entry, State x_exit, double[] ctrl, double Dt, double cost)
        {
            Edge e = new Edge();
            e.entry = x_entry;
            e.exit = x_exit;
            e.control = ctrl;
            e.step_cost = cost;
            e.Dt = Dt;

            return e;
        }

        virtual public void SetRoot(double[] q_root)
        {
            x_root = SpawnState(q_root);
            vertices.AddState(x_root);
        }

        abstract public void Execute(object input);

        virtual public void ReportStatistics(ref List<string> info)
        {
            if (info.Count < 1) info.Add(null);
            double percent = Math.Floor(n_current / (double)n_exec * 10000);
            int Z = (int)(percent / 100);
            int D = (int)(percent % 100);

            sb.Clear();
            sb.Append("progress = ").Append(Z);
            if (D == 0)
                sb.Append(".00");
            else if (D < 10)
                sb.Append(".0").Append(D);
            else
                sb.Append(".").Append(D);
            sb.Append("% = ").Append(n_current).Append(" / ").Append(n_exec);
            info[0] = sb.ToString();
        }

        public double[] AliasOf(double[] q)
        {
            double[] alias = new double[system.DOF_Alias];
            AliasOf(q, ref alias);
            return alias;
        }

        abstract public void AliasOf(double[] q, ref double[] alias);

        virtual public bool Test()
        {
            throw new NotImplementedException();
        }
    }
}
